// 雷达人感模块   默认单目标模式
// （理论上）雷达偏移补偿后，实际检测角度不足120°，且左右会有极小的误差
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stdint.h>
#include <rtthread.h>
#include <aic_core.h>
#include <rtdevice.h>
#include <math.h>
#include "pm20_app_radar.h"
#include "pm20_drv_uart3.h"
#include "rtdevice.h"
#include "aic_hal_gpio.h"
#include "pm20_app_board.h"

/******************************************************************************************************** */
extern  size_t rad_recv_buff_len;                   // 当前缓冲区中的数据长度
#define M_PI 3.14159265358979323846
#define STEP_180 6000                               // 风机180°的开度
#define RAD_RECV_BUFF_LEN 8                         // 增加缓冲区大小
#define RAD_SHORTEST_DISTANCE  3 * 1000             // 人靠近的开机距离 3m
#define X_RAD_COMPENSATION     0                    // 雷达X轴补偿距离，‘-’		毫米
#define Y_RAD_COMPENSATION     0                    // 雷达y轴补偿距离，‘+’		毫米
extern  uint8_t rad_recv_buff[RAD_RECV_BUFF_LEN];   // 接收数据缓冲区
uint16_t step_max;
uint16_t step_min;
int rad_shortest_distance;
int angle_max;
int angle_min;
int rad_state;
/******************************************************************************************************** */

double rad_calculate_angle(int x, int y) 
{
    // 使用atan2函数计算弧度值
    double radians = atan2(y, x);
    double degrees = radians * (180.0 / M_PI);  //(30-150)
    return (180 - degrees);
}

double calculateDistanceFromOrigin(int x, int y) 
{
    // 计算 x 和 y 的平方和，然后开方得到距离
    return sqrt(pow(x, 2) + pow(y, 2));
}
// 定义开雷达指令
const uint8_t openRadarCommand[] = {0xFD, 0xFC, 0xFB, 0xFA, 0x02, 0x00, 0x30, 0x00, 0x04, 0x03, 0x02, 0x01};
// 定义关雷达指令
const uint8_t closeRadarCommand[] = {0xFD, 0xFC, 0xFB, 0xFA, 0x02, 0x00, 0x31, 0x00, 0x04, 0x03, 0x02, 0x01};
// 定义单目标检测模式指令
const uint8_t simplemodeCommand[] = {0xFD, 0xFC, 0xFB, 0xFA, 0x02, 0x00, 0x80, 0x00, 0x04, 0x03, 0x02, 0x01};
// 定义多目标检测模式指令
const uint8_t multimodeCommand[] = {0xFD, 0xFC, 0xFB, 0xFA, 0x02, 0x00, 0x90, 0x00, 0x04, 0x03, 0x02, 0x01};

// 函数：解析目标数据
void parse_target_data(const uint8_t *target_data, Target *target) 
{
    if (target_data == NULL || target == NULL) 
    {
        return;
    }
    // 解析目标 x 坐标
    int16_t x_coordinate = (target_data[0] | (target_data[1] << 8));
    // 检查最高位并转换为有符号整数
    if (x_coordinate & 0x8000) {
        x_coordinate -= 0x8000; 
    }
    else {
        x_coordinate = -x_coordinate;
    }
    // 解析目标 y 坐标
    int16_t y_coordinate = (target_data[2] | (target_data[3] << 8));
    // 检查最高位并转换为有符号整数
    if (y_coordinate & 0x8000) {
        y_coordinate -= 0x8000; 
    }
    else {
        y_coordinate = -y_coordinate;
    }
    // 解析目标速度
    int16_t velocity = (target_data[4] | (target_data[5] << 8));
    // 检查最高位并转换为有符号整数
    if (velocity & 0x8000) {
        velocity -= 0x8000; 
    }
    else {
        velocity = -velocity;
    }
    // 解析目标距离采样长度
    uint16_t distance = (target_data[6] | (target_data[7] << 8));
    // 赋值到目标结构体
    target->x_coordinate = x_coordinate - X_RAD_COMPENSATION;       // 雷达坐标位置补偿
    target->y_coordinate = y_coordinate + Y_RAD_COMPENSATION;
    target->velocity = velocity;
    target->distance = distance;
}

// 函数：解析整个数据帧
void parse_data_frame(const uint8_t *frame_data, size_t frame_length, Target *targets, int num_targets) 
{
    int angle0,angle1,angle2;
    int add0,add1,add2;
    //判断雷达数据帧长度是否正确
    if (frame_length < FRAME_HEADER_LENGTH + FRAME_TAIL_LENGTH + 3 * TARGET_DATA_LENGTH) 
    {
        num_targets = 0;
        return;
    }
    //检验帧头与帧尾是否正确
    if (frame_data[0] != FRAME_HEAD1 || frame_data[1] != FRAME_HEAD2 ||
        frame_data[2] != FRAME_HEAD3 || frame_data[3] != FRAME_HEAD4 ||
        frame_data[frame_length - 2] != FRAME_END1 || frame_data[frame_length - 1] != FRAME_END2) 
    {
        num_targets = 0;
        return;
    }
    //判断是否有人
    if (frame_data[4] == 0 && frame_data[5] == 0 && frame_data[6] == 0 && frame_data[7] == 0)
    {
        num_targets = 0;
        return;
    }
    else if (frame_data[12] == 0 && frame_data[13] == 0 && frame_data[14] == 0 && frame_data[15] == 0)
    {
        num_targets = 1;
    }
    else if (frame_data[20] == 0 && frame_data[21] == 0 && frame_data[22] == 0 && frame_data[23] == 0)
    {
        num_targets = 2;
    }
    else 
    {
        num_targets = 3;
    }

    // 解析每个目标的数据
    for (int i = 0; i < num_targets; ++i) 
    {
        const uint8_t *target_data = frame_data + FRAME_HEADER_LENGTH + i * TARGET_DATA_LENGTH;
        parse_target_data(target_data, &targets[i]);
    }

    // 判断人员数量，对比坐标
    switch (num_targets)
    {
    case 0:
    {
        break;
    }
    case 1:
    {
        angle_max = rad_calculate_angle(targets[0].x_coordinate, targets[0].y_coordinate);
        rad_shortest_distance = calculateDistanceFromOrigin(targets[0].x_coordinate, targets[0].y_coordinate);
        angle_min = 0;
        break;
    }
    case 2:
    {
        angle0 = rad_calculate_angle(targets[0].x_coordinate, targets[0].y_coordinate);
        angle1 = rad_calculate_angle(targets[1].x_coordinate, targets[1].y_coordinate);
        add0 = calculateDistanceFromOrigin(targets[0].x_coordinate, targets[0].y_coordinate);
        add1 = calculateDistanceFromOrigin(targets[1].x_coordinate, targets[1].y_coordinate);
        rad_shortest_distance = (add0 <= add1) ? add0 : add1;
        if(angle0 >= angle1)
        {
            angle_max = angle0;
            angle_min = angle1;
        }
        else
        {
            angle_max = angle1;
            angle_min = angle0;
        }
        break;
    }
    case 3:
    {
        angle0 = rad_calculate_angle(targets[0].x_coordinate, targets[0].y_coordinate);
        angle1 = rad_calculate_angle(targets[1].x_coordinate, targets[1].y_coordinate);
        angle2 = rad_calculate_angle(targets[2].x_coordinate, targets[2].y_coordinate);
        add0 = calculateDistanceFromOrigin(targets[0].x_coordinate, targets[0].y_coordinate);
        add1 = calculateDistanceFromOrigin(targets[1].x_coordinate, targets[1].y_coordinate);
        add2 = calculateDistanceFromOrigin(targets[2].x_coordinate, targets[2].y_coordinate);
        rad_shortest_distance = (add0 <= ((add1 <= add2) ? add1 : add2)) ? add0 : ((add1 <= add2) ? add1 : add2);
        angle_max = (angle0 >= ((angle1 >= angle2) ? angle1 : angle2)) ? angle0 : ((angle1 >= angle2) ? angle1 : angle2);
        angle_min = (angle0 <= ((angle1 <= angle2) ? angle1 : angle2)) ? angle0 : ((angle1 <= angle2) ? angle1 : angle2);
        break;
    }
    default:
        break;
    }

    // 靠近开机模式开启，并且雷达检测到人，并且机器处于关闭状态
    if(rad_state == 1 && num_targets != 0 && send_frame.power_on_command == 0)
    {
        // 雷达检测到人的距离小于3m，机器开启
        if(rad_shortest_distance < RAD_SHORTEST_DISTANCE)
        {
            send_frame.power_on_command = 1;
            rt_pin_write(rt_pin_get("PC.7"), PIN_HIGH);
        }
    }

    step_max = (uint16_t)(STEP_180 / 180 * angle_max);
    step_min = (uint16_t)(STEP_180 / 180 * angle_min);
    
	printf("MAX ANGLE = %d\n",angle_max);
    printf("MIN ANGLE = %d\n",angle_min);
    printf("MAX STEP = %d\n",step_max);
    printf("MIN STEP = %d\n",step_min);
    printf("The unsigned 16-bit MAX STEP value is: %04X\n", step_max);
    printf("The unsigned 16-bit MIN STEP value is: %04X\n", step_min);

    int thousands, hundreds, tens, ones, tenths, hundredths;
    thousands = rad_shortest_distance / 1000;          // 获取千位
    hundreds = (rad_shortest_distance % 1000) / 100;   // 获取百位
    tens = (rad_shortest_distance % 100) / 10;         // 获取十位
    ones = rad_shortest_distance % 10;                 // 获取个位
    tenths = hundreds * 10 + tens;   // 十位和百位组合成十分位
    hundredths = ones;          // 个位乘以10得到百分位
    printf("The nearest distance from the radar is:%d.%d%d m\n", thousands, tenths, hundredths);

    // 打印解析结果
    for (int i = 0; i < num_targets; ++i)
    {
        printf("%d:\n", i + 1);
        printf("%d cm\n", targets[i].x_coordinate / 10);
        printf("%d cm\n", targets[i].y_coordinate / 10);
        printf("%d cm/s\n", targets[i].velocity);
        // printf("%d mm\n", targets[i].distance);
    }
}

// 靠近开机模式的接口     1打开  0关闭
void colse_to_boot(int state) 
{
    rad_state = state;
}

// 风随人动转机开度上传接口
uint16_t rad_get_step_max(void)
{
    return step_max;
}
uint16_t rad_get_step_min(void)
{
    return step_min;
}
/*********************************************************************************************************/
static void pm_app_radar_callback(void *parameter) 
{
    send_rad_command(openRadarCommand);
    // 开启多目标模式
    send_rad_command(multimodeCommand);
    while (1) 
    {
        const uint8_t *frame_data = process_received_rad_data();
        uint16_t frame_length = get_rad_recv_buff_len();
        
        Target *targets = NULL; // 初始化指针为NULL
        int num_targets; // 存储实际解析到的目标数量

        targets = (Target *)malloc(MAX_TARGETS * sizeof(Target));
        if (targets == NULL) 
        {
            printf("Failed to allocate memory for targets.\n");
        }
        // 遍历数组并打印每个元素
        for (int i = 0; i < frame_length; ++i) 
        {
            printf("%02X ", frame_data[i]);
        }
        printf("\n"); // 打印换行符
        if (frame_length == 30)
        {
            parse_data_frame(frame_data, frame_length, targets, num_targets);// 解析雷达数据帧
        } 
        rad_recv_buff_len = 0;
        rt_thread_mdelay(1000); //延时1s ,1s刷新一次
        free(targets);// 释放targets指针内存
        targets = NULL; // 将指针重置为NULL
    }
}
void pm20_app_radar_init(void)
{
    rt_thread_t rader = rt_thread_create("rader", pm_app_radar_callback, RT_NULL, 1024*4, 25, 10);
    if(rader != RT_NULL)
    {
        rt_thread_startup(rader);
    }
    else
    {
        printf("err\n");
    }
}